#include "improc_render.h"
#include <opencv2/imgproc/imgproc.hpp>

using namespace std;
using namespace cv;

#define size_H 480
#define size_W 640


void ImprocRender::process( cv::Mat& color_frame, long delta_time )
{
	drawImageOverlappedSensor( color_frame, *_color_img, *_mask, _marker_width, _marker_height );
}


void ImprocRender::drawImageOverlappedSensor(  cv::Mat& colorImage , cv::Mat& imageToOverlap, cv::Mat& imageToOverlapMask, int marker_width ,int marker_height )
{
	if (!_objectFound)
		return;

	cv::Point c( (int)(30), (int)(50) );
	cv::Scalar co = cv::Scalar(1*255,0.5*255,0.5*255,255);

	std::vector<cv::Point3f> _objPoints3dToDraw;

	int hMarker = marker_height;//_marker->rows;
	int wMarker = marker_width;//_marker->cols;

	int hOverImg = imageToOverlap.rows;
	int wOverImg = imageToOverlap.cols;
	float aspectOverImg  = (float)wOverImg / (float)hOverImg;

	float hScaled = (float)wMarker / aspectOverImg;
	float wScaled = (float)wMarker;

	if(hScaled > hMarker)
	{
		wScaled = (float)hMarker * aspectOverImg;
		hScaled = (float)hMarker;
	}

	_objPoints3dToDraw.push_back(cv::Point3f(0.0f,0.0f,0.0f));	
	_objPoints3dToDraw.push_back(cv::Point3f(wScaled,0.0f,0.0f));
	_objPoints3dToDraw.push_back(cv::Point3f(wScaled,hScaled,0.0f));
	_objPoints3dToDraw.push_back(cv::Point3f(0.0f,hScaled,0.0f));

	std::vector<cv::Point2f> Points2dOverImg;

	Points2dOverImg.push_back(cv::Point2f(0.0f,0.0f));	
	Points2dOverImg.push_back(cv::Point2f((float)wOverImg,0.0f));
	Points2dOverImg.push_back(cv::Point2f((float)wOverImg,(float)hOverImg));
	Points2dOverImg.push_back(cv::Point2f(0.0f,(float)hOverImg));

	std::vector<cv::Point2f> imagePoints2;
	imagePoints2.resize( 4 );
	float zTmp;
	pointProject( 0.0f   , 0.0f   , 0.0f, &imagePoints2[0].x,  (&imagePoints2[0].y), &zTmp );	
	pointProject( wScaled, 0.0f   , 0.0f, &imagePoints2[1].x,  (&imagePoints2[1].y), &zTmp );	
	pointProject( wScaled, hScaled, 0.0f, &imagePoints2[2].x,  (&imagePoints2[2].y), &zTmp );	
	pointProject( 0.0f   , hScaled, 0.0f, &imagePoints2[3].x,  (&imagePoints2[3].y), &zTmp );

	/*printf("\ns 0.0 0.0 0.0 %.1f %.1f %.1f\n",imagePoints2[0].x,480.0f -  (imagePoints2[0].y), zTmp);
	printf("s 0.0 0.0 0.0 %.1f %.1f %.1f\n",imagePoints2[1].x,480.0f -  (imagePoints2[1].y), zTmp);
	printf("s 0.0 0.0 0.0 %.1f %.1f %.1f\n",imagePoints2[2].x,480.0f -  (imagePoints2[2].y), zTmp);
	printf("s 0.0 0.0 0.0 %.1f %.1f %.1f\n\n\n",imagePoints2[3].x,480.0f -  (imagePoints2[3].y), zTmp);
*/
	imagePoints2[0].y = size_H - imagePoints2[0].y;
	imagePoints2[1].y = size_H - imagePoints2[1].y;
	imagePoints2[2].y = size_H - imagePoints2[2].y;
	imagePoints2[3].y = size_H - imagePoints2[3].y;

	 cv::Mat overTo3DMatrix = getPerspectiveTransform(Points2dOverImg,imagePoints2);
	 cv::Mat _imageWarped =  cv::Mat::zeros(colorImage.cols,colorImage.rows,colorImage.type());
	 cv::Mat mask(colorImage.cols,colorImage.rows,CV_8U);
	
	warpPerspective(imageToOverlap,_imageWarped,overTo3DMatrix,cv::Size(colorImage.cols,colorImage.rows));
	warpPerspective(imageToOverlapMask,mask,overTo3DMatrix,cv::Size(colorImage.cols,colorImage.rows));
	
	_imageWarped.copyTo(colorImage,mask);
	

	for (unsigned int i = 0; i < _objPoints3dToDraw.size(); i++)
	{
		cv::Point center( (int)(imagePoints2[i].x), (int)(imagePoints2[i].y) );		
		cv::Scalar color = cv::Scalar(_objPoints3dToDraw[i].x*6,_objPoints3dToDraw[i].y*6,_objPoints3dToDraw[i].z*6,255);
		cv::circle( colorImage, center, 3, color);
	}
}



void ImprocRender::pointProject( float objX, float objY, float objZ, float* winX, float* winY, float* winZ )
{
	int viewport[4];
	viewport[0] = 0;
	viewport[1] = 0;
	viewport[2] = 640;
	viewport[3] = 480;	
	
	//getGLViewMatrix(mv);
	//getGLProjectionMatrix(proj);
	//double wX,wY,wZ;
	//gluProject(objX,objY,objZ,mv,proj,viewport,&wX,&wY,&wZ);

	float in[4];
	in[0] = _mvp[0] * objX +  _mvp[4] * objY + _mvp[8]  * objZ + _mvp[12];
	in[1] = _mvp[1] * objX +  _mvp[5] * objY + _mvp[9]  * objZ + _mvp[13];
	in[2] = _mvp[2] * objX +  _mvp[6] * objY + _mvp[10] * objZ + _mvp[14];
	in[3] = _mvp[3] * objX +  _mvp[7] * objY + _mvp[11] * objZ + _mvp[15];

	if (in[3] == 0) return;

	in[3] = 1.0f/in[3];

	in[0] *= in[3];
	in[1] *= in[3];
	in[2] *= in[3];

	in[0] = in[0] * 0.5f + 0.5f;
	in[1] = in[1] * 0.5f + 0.5f;
	in[2] = in[2] * 0.5f + 0.5f;



	in[0] = in[0] * viewport[2] + viewport[0];
	in[1] = in[1] * viewport[3] + viewport[1];

	*winX = in[0];
	*winY = in[1];
	*winZ = in[2];
}


void ImprocRender::setModelViewProjection( float* mvp )
{
	memcpy(_mvp,mvp,sizeof(float)*16);
}

ImprocRender::ImprocRender()
{
	_objectFound = false;
}

void ImprocRender::setObjectFound( bool v )
{
	_objectFound = v;
}

void ImprocRender::setOverlapImage( cv::Mat* color_img,cv::Mat* mask, int marker_width ,int marker_height )
{
	_marker_width = marker_width;
	_marker_height = marker_height;
	_color_img = color_img;
	_mask = mask;
}

